/**
  ******************************************************************************
  * File Name          : motor.c
  * Description        : This file provides code for the configuration
  *                      of the TiAxis instances.
  ******************************************************************************
  * @attention
  * Created by Chen Lu in 20210527
  *
  ******************************************************************************
  */

#include "stdlib.h"
#include "string.h"
#include "axis.h"
#include "motor.h"

/**
  * @brief  Open an axis.
  *
  * @note   This function creates an axis instance.
  *
  * @param  id	The axis id number.
  * @retval An axis instance pointer.
  */
TiAxis * axis_open(uint8_t id)
{
	TiAxis *axis = (TiAxis*) malloc(sizeof(TiAxis));
	memset(axis, 0x00, sizeof(TiAxis));
	axis->id = id;
	axis->motor = motor_open(id);
	axis->max_angle = MAX_ANGLE;
	axis->encoder = 0;
	axis->zero_flag = 0;
	return axis;
}

/**
  * @brief  Close an axis.
  *
  * @note   This function closes a axis instance
  *
  * @param  axis	The axis instance pointer.
  * @retval AXIS status.
  */
uint8_t axis_close(TiAxis * axis)
{
	if (motor_close(axis->motor) != MOTOR_OK)
	{
		printf("%s\r\n","Axis close error!");
		return AXIS_ERROR;
	}
	return AXIS_OK;
}

/**
  * @brief  Read the pulses an axis has stepped(accumulated by 'P'&'N' direction).
  *
  * @note   This function read the angle an axis has rotated. However, here we use pulses PWM outputs instead,
  * 		cause there is no physical encoder.
  *
  * @param  axis	The axis instance pointer.
  * @retval Number of pulses.
  */
int axis_read(TiAxis * axis)
{
	if (axis->encoder > axis->max_angle)
	{
		printf("%s\r\n","Axis rotation out of range!");
		return AXIS_ERROR;
	}
	return axis->encoder;
}

/**
  * @brief  Configure an axis instance.
  *
  * @note   This function read the angle an axis has rotated. However, here we use pulses PWM outputs instead,
  * 		cause there is no physical encoder.
  *
  * @param  axis	The axis instance pointer.
  * @retval Number of pulses.
  */
uint8_t axis_write(TiAxis * axis, uint8_t direction, uint16_t speed)
{
	if (motor_write(axis->motor,direction,speed) != MOTOR_OK)
	{
		printf("%s\r\n","Axis write error!");
		return AXIS_ERROR;
	}

	return AXIS_OK;
}

/**
  * @brief  Start an axis.
  *
  * @note   This function starts an axis.
  *
  * @param  axis	The axis instance pointer.
  * @retval AXIS status.
  */
uint8_t axis_start(TiAxis * axis)
{
	//Can't be deleted. It avoids HardFault when the axis speed is really 0.
	if (axis->motor->speed == 0) return AXIS_OK;
	if (motor_start(axis->motor) != MOTOR_OK)
	{
		printf("%s\r\n","Axis start error!");
		return AXIS_ERROR;
	}
	return AXIS_OK;
}

/**
  * @brief  Stop an axis.
  *
  * @note   (Nothing important)
  *
  * @param  axis	The axis instance pointer.
  * @retval AXIS status.
  */
uint8_t axis_stop(TiAxis * axis)
{
	if (motor_stop(axis->motor) != MOTOR_OK)
	{
		printf("%s\r\n","Axis stop error!");
		return AXIS_ERROR;
	}
	return AXIS_OK;
}

/**
  * @brief  Return an axis to zero.
  *
  * @note   This function return an axis to zero. Note that axis[0] in the project does not have a zero adjuster,
  * 		so it need to be adjusted manually.
  *
  * @param  axis	The axis instance pointer.
  * @retval AXIS status.
  */
uint8_t axis_returnzero(TiAxis * axis)
{
	if (axis->zero_flag == 1) return AXIS_OK;

	if (axis_write(axis, 'N', 150) != AXIS_OK)
	{
		printf("%s\r\n","Axis return zero write error!");
		return AXIS_ERROR;
	}
	if (axis_start(axis) != AXIS_OK)
	{
		printf("%s\r\n","Axis return zero start error!");
		return AXIS_ERROR;
	}

	while(1)
	{
		if (axis->zero_flag == 1)
		{
			if (axis_stop(axis) != AXIS_OK)
			{
				printf("%s\r\n","Axis return zero stop error!");
				return AXIS_ERROR;
			}
			axis->encoder = 0;
			break;
		}
	}
	return AXIS_OK;
}

